#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "Servo.h"
#include "Key.h"
#include "Serial.h"
#include "string.h"
#include "opencv.h"
#include "PID.h"           				// 确保包含PID头文件

uint8_t RxData;            				// 定义用于接收串口数据的变量
int opencv[7];             				// stm32接收数据数组
int16_t data1;
int16_t data2;
int16_t data3;
int16_t data4;

// 摄像头图像尺寸  
#define CAMERA_WIDTH 640  
#define CAMERA_HEIGHT 480  

// 小球位置与偏差
float ballX = CAMERA_WIDTH / 2; 		// 小球在图像中的x位置  
float ballY = CAMERA_HEIGHT / 2; 		// 小球在图像中的y位置
float x,y;
float deltaX, deltaY; 					// 与中心的偏差值

// OLED显示控制	
uint8_t dispRefreshCnt = 0;  			// 显示刷新计数器
#define DISP_REFRESH_THRESH 5  			// 每5次循环刷新一次（约100Hz）

// 舵机与PID参数
float Angle;            				//定义角度变量
PID_Controller panPID;  
PID_Controller tiltPID;
float servo_rotation_value=90.0;
float servo_pitch_value=60.0;
uint8_t servo_rotation_direction=0;
uint8_t servo_pitch_direction=1;


int main(void)
{
    /*模块初始化*/
    Servo_Init();        				//舵机初始化
    OLED_Init();
    OLED_Clear();  						// 清屏
    uart_init(115200);  
    OLED_ShowString(1,1,"Init OK");  	//初始化提示
    Delay_ms(1000);  					// 显示1秒
    
    // 初始化PID控制器（调整参数，添加微分作用）
    PID_Init(&panPID,  0.00095f, 0.000f, 0.0001f); 		// 水平轴PID参数  
    PID_Init(&tiltPID, 0.00095f, 0.000f, 0.0001f); 		// 垂直轴PID参数  
    PID_SetSetpoint(&panPID,CAMERA_WIDTH/2);  			// 水平轴目标：图像中心X
    PID_SetSetpoint(&tiltPID,CAMERA_HEIGHT/2); 			// 垂直轴目标：图像中心Y
//    Angle=90;  										// 角度初始值
    
    // 舵机初始位置设置
    Servo_SetAngle1(servo_rotation_value);  			// 水平舵机初始角度（90度）
    Servo_SetAngle2(servo_pitch_value);     			// 垂直舵机初始角度（60度）
    Delay_ms(1000);  									// 等待舵机到位
    
    OLED_Clear();  										// 清屏，准备显示实时数据
    // 只初始化显示一次标题（减少刷新）
    OLED_ShowString(1,1,"X:");
    OLED_ShowString(2,1,"Y:");
    OLED_ShowString(3,1,"dX:");
    OLED_ShowString(4,1,"dY:");
    
    while (1)
    {
        // 更新小球位置
        ballX=(uint16_t)data1;  						// 使用uint16_t避免数据截断
        ballY=(uint16_t)data2;
        
        // 计算与图像中心的偏差
        deltaX = ballX - CAMERA_WIDTH / 2;  
        deltaY = ballY - CAMERA_HEIGHT / 2;  
        
        // 异常处理：未检测到目标时，默认中心位置
        if(ballX==0 || ballX > CAMERA_WIDTH || ballY ==0 || ballY > CAMERA_HEIGHT) {
            ballX=320;  
            ballY=240;  
            deltaX = 0;  
            deltaY = 0;  
        }
        
        // 控制OLED刷新频率
        dispRefreshCnt++;
        if(dispRefreshCnt >= DISP_REFRESH_THRESH)
        {
            dispRefreshCnt = 0;  						// 重置计数器
            // 刷新显示数据
            OLED_ShowNum(1,3,(uint16_t)ballX,5);  
            OLED_ShowNum(2,3,(uint16_t)ballY,5);  
            OLED_ShowSignedNum(3,4,(int)deltaX,4);  
            OLED_ShowSignedNum(4,4,(int)deltaY,4);
			Delay_ms(10);
        }
        
        // PID计算舵机调整量
        x=PID_Compute(&panPID, ballX);  				// 水平轴调整量
        y=PID_Compute(&tiltPID, ballY); 				// 垂直轴调整量
            
        // 舵机方向修正（修复逻辑错误）
        if (servo_rotation_direction) { 
            x = -x; 									// 根据方向标志反转控制量
        }  
        if (servo_pitch_direction) { 
            y = y; 										// 根据方向标志反转控制量
        }  
            
        // 水平舵机限位控制
        if (servo_rotation_value + x > MIN_ANGLE_X && servo_rotation_value + x < MAX_ANGLE_X) {  
            servo_rotation_value += x;  
            Servo_SetAngle1(servo_rotation_value);  
        }  
            
        // 垂直舵机限位控制
        if (servo_pitch_value + y > MIN_ANGLE_Y && servo_pitch_value + y < MAX_ANGLE_Y) {  
            servo_pitch_value += y;  
            Servo_SetAngle2(servo_pitch_value);  
        }
            
        Delay_ms(5);  									// 延时（约200Hz控制频率）
    }
}
